/*
 * Matrix.cpp
 *
 *  Created on: Feb 15, 2011
 *      Author: Anton
 */

#include <motion/Matrix.h>
#include <stdio.h>

#define DEG2RAD 0.0174532925
#define RAD2DEG 57.2957795

Matrix::Matrix() {
}

Matrix::Matrix(const Matrix &m) {
	dx = m.dx;
	dy = m.dy;

	array = new double*[dx];
	for (int i = 0; i < dx; i++) {
		array[i] = new double[dy];
	}

	for(int i=0; i<dx; i++) {
		for(int j=0; j<dy; j++) {
			array[i][j] = m.array[i][j];
		}
	}
}

Matrix::Matrix(int size1, int size2, ...) {
	va_list arglist;
	va_start(arglist, size2);

	dx = size1;
	dy = size2;

	array = new double*[dx];
	for (int i = 0; i < dx; i++) {
		array[i] = new double[dy];
	}

	for (int i = 0; i < size1; i++) {
		for (int j = 0; j < size2; j++) {
			array[i][j] = va_arg(arglist, double);
		}
	}

	va_end(arglist);
}

Matrix::~Matrix() {
	delete array;
	array=NULL;
	/*
	//cout << "\t\t\t\tDeleting Matrix" << endl;
	// MAX SIZE OF A MATRIX USED
	if ((dx > 4) || (dy > 4) || (dx == 0) || (dy == 0)) {
		array = NULL;
	}
	// Destroy an actual matrix.
	if (array != NULL) {
		for (int i = 0; i < dy; i++) {
			delete[] array[i];
		}
		delete array;
	}
	//cout << "Matrix Destroyed" << endl;
	 */
}

void Matrix::resize(int sizeX, int sizeY) {
	if (array != NULL) {
		//delete array;
		array = NULL;
	}
	array = new double*[sizeX];
	for (int i = 0; i < sizeX; i++)
		array[i] = new double[sizeY];
}

void Matrix::setvalue(int x, int y, double val) {
	array[x][y] = val;
}

double Matrix::getvalue(int x, int y) {
	return array[x][y];
}

void Matrix::print() {
	if((dx > 0) && (dy > 0)){
		for (int i = 0; i < dx; i++) {
			cout << "[ ";
			for (int j = 0; j < dy; j++) {
				cout << array[i][j] << " ";
			}
			cout << "]" << endl;
		}
		}
	else{
		cout << "null matrix" << endl;
	}
}

int Matrix::getX() {
	return dx;
}

int Matrix::getY() {
	return dy;
}

ostream &operator<<(ostream &stream, Matrix &m) {

	for (int i = 0; i < m.dx; i++) {
		stream << "[ ";
		for (int j = 0; j < m.dy; j++) {
			stream << m.array[i][j] << " ";
		}
		stream << "]" << endl;
	}
	stream << endl;

	return stream;
}

Matrix& Matrix::operator=(const Matrix& rhs) {
	dx = rhs.dx;
	dy = rhs.dy;
	for (int i = 0; i < rhs.dx; i++) {
		for (int j = 0; j < rhs.dy; j++) {
			array[i][j] = rhs.array[i][j];
		}
	}
	return *(this);
}

Matrix operator*(Matrix& lhs, Matrix& rhs) {
	//ret is AxC size matrix
	//3x4 Matrix has 3 rows, 4 columns
	//dy, columns - dx, rows
	//(AxB)*(BxC) = (AxC)

	//Matrix:: is AxB
	//m is BxC

	Matrix ret = Matrix(rhs.dy, lhs.dx);

	if (lhs.dy == rhs.dx) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				//Multiply/Add
				double value = 0;
				for (int k = 0; k < lhs.dy; k++) {
					value = value + lhs.getvalue(i, k) * rhs.getvalue(k, j);
				}
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	//ret.print();

	return ret;
}

Matrix operator-(Matrix& lhs, Matrix& rhs) {
	Matrix ret = Matrix(rhs.dy, rhs.dx);

	if ((lhs.dy == rhs.dy) & (lhs.dx == rhs.dx)) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				double value = lhs.getvalue(i, j) - rhs.getvalue(i, j);
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	return ret;
}

Matrix operator+(Matrix& lhs, Matrix& rhs) {
	Matrix ret = Matrix(rhs.dy, rhs.dx);

	if ((lhs.dy == rhs.dy) & (lhs.dx == rhs.dx)) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				double value = lhs.getvalue(i, j) + rhs.getvalue(i, j);
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	return ret;
}

Matrix Matrix::transMatrix(){
	Matrix T = Matrix(4,4, 1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0);
	if(Matrix::getX()==1 && Matrix::getY()==4){
		double theta=Matrix::getvalue(0,3);
		double a=Matrix::getvalue(0,0);
		double alpha=Matrix::getvalue(0,1);
		double d=Matrix::getvalue(0,2);

		//Transformation Matrix utilizing DH Params
		T.setvalue(0,0,cos(theta));
		T.setvalue(0,1,-sin(theta)*cos(alpha));
		T.setvalue(0,2,sin(alpha)*sin(theta));
		T.setvalue(0,3,a*cos(theta));

		T.setvalue(1,0,sin(theta));
		T.setvalue(1,1,cos(alpha)*cos(theta));
		T.setvalue(1,2,-cos(theta)*sin(alpha));
		T.setvalue(1,3,a*sin(theta));

		T.setvalue(2,0,0.0);
		T.setvalue(2,1,sin(alpha));
		T.setvalue(2,2,cos(alpha));
		T.setvalue(2,3,d);
		//cout << "TransformMatrix: " << T << endl;
	}
	else{
		cout << "Error: Improper Matrix dimensions." << endl;
	}
	return T;
}
